import { Node, Label, Layers, Prefab, instantiate, Color, Button, PhysicsSystem, PhysicsSystem2D, Sprite, tween, Vec3 } from 'cc';
import { Vector2, Vector3 } from '../utils/Math';
import { RVOManager } from '../rvo/RVOManager';
import { PoolManager } from '../managers/PoolManager';
import { Collision } from '../collision/Collision';

/**
 * RVO系统测试用例，节点可视化、自动运动、碰撞高亮、对象池复用
 */
export class RVOTest {
    /**
     * 2D RVO批量测试：每个agent自动创建节点，运动、碰撞高亮、对象池复用
     * label默认绿色，碰撞红色
     */
    static test2DBatch(parentNode: Node, prefab?: Prefab, agentCount = 10) {
        if (PhysicsSystem2D && !PhysicsSystem2D.instance.enable) PhysicsSystem2D.instance.enable = true;
        const rvo = new RVOManager('2d');
        rvo.addObstacle({ p1: new Vector2(0, 0), p2: new Vector2(10, 0) });
        const agentNodes: { id: number, node: Node, label: Label }[] = [];
        const colorList = [Color.GREEN, Color.BLUE, Color.YELLOW, Color.CYAN, Color.MAGENTA];
        for (let i = 0; i < agentCount; i++) {
            const pos = new Vector2(0, 2 + i * 0.8);
            const goal = new Vector2(10, 2 + i * 0.8);
            const id = rvo.addAgent({ position: pos, radius: 0.5, maxSpeed: 1, goal });
            let node: Node;
            if (prefab) {
                node = PoolManager.instance.getNode('RVOAgent2D', prefab);
            } else {
                node = new Node('RVOAgent2D');
            }
            node.layer = Layers.Enum.UI_2D;
            node.setPosition(pos.x * 10, pos.y * 10);
            let label = node.getComponent(Label);
            if (!label) {
                label = node.addComponent(Label);
                label.fontSize = 18;
                label.lineHeight = 24;
                label.color = colorList[i % colorList.length];
            }
            parentNode.addChild(node);
            agentNodes.push({ id, node, label });
        }
        // 障碍物节点（灰色）
        const obsNode = new Node('Obstacle');
        obsNode.layer = Layers.Enum.UI_2D;
        obsNode.setPosition(50, 0);
        const obsLabel = obsNode.addComponent(Label);
        obsLabel.string = '障碍';
        obsLabel.fontSize = 20;
        obsLabel.color = Color.GRAY;
        parentNode.addChild(obsNode);
        // 目标点节点（紫色）
        for (let i = 0; i < agentCount; i++) {
            const goalNode = new Node('Goal' + i);
            goalNode.layer = Layers.Enum.UI_2D;
            goalNode.setPosition(100, 20 + i * 8);
            const goalLabel = goalNode.addComponent(Label);
            goalLabel.string = '目标';
            goalLabel.fontSize = 16;
            goalLabel.color = Color.MAGENTA;
            parentNode.addChild(goalNode);
        }
        let frame = 0;
        const update = () => {
            rvo.update();
            for (let idx = 0; idx < agentNodes.length; idx++) {
                const { id, node, label } = agentNodes[idx];
                const agent = rvo.getAgent(id);
                if (!agent) continue;
                const pos = agent.position;
                node.setPosition(pos.x * 10, pos.y * 10);
                // 碰撞高亮
                const hit = Collision.circleCircle(pos, agent.radius, new Vector2(0, 0), 0.5);
                label.string = `Agent${id} (${pos.x.toFixed(2)},${pos.y.toFixed(2)})${hit ? ' 碰撞' : ''}`;
                label.color = hit ? Color.RED : colorList[idx % colorList.length];
            }
            frame++;
            if (frame < 120) {
                setTimeout(update, 50);
            } else {
                for (const { node } of agentNodes) {
                    if (prefab) {
                        PoolManager.instance.putNode('RVOAgent2D', node);
                    } else {
                        node.removeFromParent();
                        node.destroy();
                    }
                }
                obsNode.removeFromParent(); obsNode.destroy();
                // 移除目标点
                for (let i = 0; i < agentCount; i++) {
                    const goalNode = parentNode.getChildByName('Goal' + i);
                    if (goalNode) goalNode.removeFromParent();
                }
            }
        };
        update();
    }
    /**
     * 2D RVO动态障碍物避让测试
     */
    static test2DDynamicObstacle(parentNode: Node, agentCount = 5) {
        if (PhysicsSystem2D && !PhysicsSystem2D.instance.enable) PhysicsSystem2D.instance.enable = true;
        const rvo = new RVOManager('2d');
        // 动态障碍物
        let obsPos = new Vector2(5, 5);
        rvo.addObstacle({ p1: obsPos, p2: new Vector2(5, 15) });
        const obsNode = new Node('DynObstacle');
        obsNode.layer = Layers.Enum.UI_2D;
        obsNode.setPosition(obsPos.x * 10, obsPos.y * 10);
        const obsLabel = obsNode.addComponent(Label);
        obsLabel.string = '动态障碍';
        obsLabel.fontSize = 20;
        obsLabel.color = Color.GRAY;
        parentNode.addChild(obsNode);
        // 智能体
        const agentNodes: { id: number, node: Node, label: Label }[] = [];
        const colorList = [Color.GREEN, Color.BLUE, Color.YELLOW, Color.CYAN, Color.MAGENTA];
        for (let i = 0; i < agentCount; i++) {
            const pos = new Vector2(0, 6 + i * 2);
            const goal = new Vector2(10, 6 + i * 2);
            const id = rvo.addAgent({ position: pos, radius: 0.5, maxSpeed: 1, goal });
            const node = new Node('RVOAgent2D');
            node.layer = Layers.Enum.UI_2D;
            node.setPosition(pos.x * 10, pos.y * 10);
            const label = node.addComponent(Label);
            label.fontSize = 18;
            label.lineHeight = 24;
            label.color = colorList[i % colorList.length];
            parentNode.addChild(node);
            agentNodes.push({ id, node, label });
        }
        let frame = 0;
        const update = () => {
            // 动态移动障碍物
            obsPos.x = 5 + Math.sin(frame * 0.05) * 3;
            obsNode.setPosition(obsPos.x * 10, obsPos.y * 10);
            // 更新障碍物在RVO中的位置（如需支持动态障碍，需RVOManager支持动态更新）
            // rvo.updateObstacle(0, { p1: obsPos, p2: new Vector2(obsPos.x, 15) });
            rvo.update();
            for (let idx = 0; idx < agentNodes.length; idx++) {
                const { id, node, label } = agentNodes[idx];
                const agent = rvo.getAgent(id);
                if (!agent) continue;
                const pos = agent.position;
                node.setPosition(pos.x * 10, pos.y * 10);
                // 碰撞高亮
                const hit = Math.abs(pos.x - obsPos.x) < 0.5 && Math.abs(pos.y - obsPos.y) < 5;
                label.string = `Agent${id} (${pos.x.toFixed(2)},${pos.y.toFixed(2)})${hit ? ' 碰撞' : ''}`;
                label.color = hit ? Color.RED : colorList[idx % colorList.length];
            }
            frame++;
            if (frame < 120) {
                setTimeout(update, 50);
            } else {
                for (const { node } of agentNodes) {
                    node.removeFromParent(); node.destroy();
                }
                obsNode.removeFromParent(); obsNode.destroy();
            }
        };
        update();
    }
    /**
     * 2D RVO多目标交互测试
     */
    static test2DMultipleGoals(parentNode: Node, agentCount = 6) {
        if (PhysicsSystem2D && !PhysicsSystem2D.instance.enable) PhysicsSystem2D.instance.enable = true;
        const rvo = new RVOManager('2d');
        // 多目标点
        const goals = [new Vector2(10, 2), new Vector2(10, 8), new Vector2(10, 14), new Vector2(0, 2), new Vector2(0, 8), new Vector2(0, 14)];
        const agentNodes: { id: number, node: Node, label: Label, goal: Vector2 }[] = [];
        const colorList = [Color.GREEN, Color.BLUE, Color.YELLOW, Color.CYAN, Color.MAGENTA, Color.YELLOW];
        for (let i = 0; i < agentCount; i++) {
            const pos = i < 3 ? new Vector2(0, 2 + i * 6) : new Vector2(10, 2 + (i - 3) * 6);
            const goal = goals[i];
            const id = rvo.addAgent({ position: pos, radius: 0.5, maxSpeed: 1, goal });
            const node = new Node('RVOAgent2D');
            node.layer = Layers.Enum.UI_2D;
            node.setPosition(pos.x * 10, pos.y * 10);
            const label = node.addComponent(Label);
            label.fontSize = 18;
            label.lineHeight = 24;
            label.color = colorList[i % colorList.length];
            parentNode.addChild(node);
            agentNodes.push({ id, node, label, goal });
            // 目标点节点
            const goalNode = new Node('Goal' + i);
            goalNode.layer = Layers.Enum.UI_2D;
            goalNode.setPosition(goal.x * 10, goal.y * 10);
            const goalLabel = goalNode.addComponent(Label);
            goalLabel.string = '目标';
            goalLabel.fontSize = 16;
            goalLabel.color = Color.MAGENTA;
            parentNode.addChild(goalNode);
        }
        let frame = 0;
        const update = () => {
            rvo.update();
            for (let idx = 0; idx < agentNodes.length; idx++) {
                const { id, node, label, goal } = agentNodes[idx];
                const agent = rvo.getAgent(id);
                if (!agent) continue;
                const pos = agent.position;
                node.setPosition(pos.x * 10, pos.y * 10);
                // 到达目标高亮
                const arrived = pos.subtract(goal).length() < 0.5;
                label.string = `Agent${id} (${pos.x.toFixed(2)},${pos.y.toFixed(2)})${arrived ? ' 到达' : ''}`;
                label.color = arrived ? Color.RED : colorList[idx % colorList.length];
            }
            frame++;
            if (frame < 120) {
                setTimeout(update, 50);
            } else {
                for (const { node } of agentNodes) {
                    node.removeFromParent(); node.destroy();
                }
                for (let i = 0; i < agentCount; i++) {
                    const goalNode = parentNode.getChildByName('Goal' + i);
                    if (goalNode) goalNode.removeFromParent();
                }
            }
        };
        update();
    }
    /**
     * 2D RVO目标动态变化测试
     */
    static test2DDynamicGoal(parentNode: Node, agentCount = 5) {
        if (PhysicsSystem2D && !PhysicsSystem2D.instance.enable) PhysicsSystem2D.instance.enable = true;
        const rvo = new RVOManager('2d');
        const agentNodes: { id: number, node: Node, label: Label }[] = [];
        const colorList = [Color.GREEN, Color.BLUE, Color.YELLOW, Color.CYAN, Color.MAGENTA];
        for (let i = 0; i < agentCount; i++) {
            const pos = new Vector2(0, 2 + i * 2);
            const goal = new Vector2(10, 2 + i * 2);
            const id = rvo.addAgent({ position: pos, radius: 0.5, maxSpeed: 1, goal });
            const node = new Node('RVOAgent2D');
            node.layer = Layers.Enum.UI_2D;
            node.setPosition(pos.x * 10, pos.y * 10);
            const label = node.addComponent(Label);
            label.fontSize = 18;
            label.lineHeight = 24;
            label.color = colorList[i % colorList.length];
            parentNode.addChild(node);
            agentNodes.push({ id, node, label });
        }
        let frame = 0;
        const update = () => {
            // 动态改变目标点
            for (let idx = 0; idx < agentNodes.length; idx++) {
                const agent = rvo.getAgent(agentNodes[idx].id);
                if (!agent) continue;
                agent.goal = new Vector2(10 + Math.sin(frame * 0.05 + idx) * 5, 2 + idx * 2);
            }
            rvo.update();
            for (let idx = 0; idx < agentNodes.length; idx++) {
                const { id, node, label } = agentNodes[idx];
                const agent = rvo.getAgent(id);
                if (!agent) continue;
                const pos = agent.position;
                node.setPosition(pos.x * 10, pos.y * 10);
                // 到达目标高亮
                const arrived = pos.subtract(agent.goal).length() < 0.5;
                label.string = `Agent${id} (${pos.x.toFixed(2)},${pos.y.toFixed(2)})${arrived ? ' 到达' : ''}`;
                label.color = arrived ? Color.RED : colorList[idx % colorList.length];
            }
            frame++;
            if (frame < 120) {
                setTimeout(update, 50);
            } else {
                for (const { node } of agentNodes) {
                    node.removeFromParent(); node.destroy();
                }
            }
        };
        update();
    }
} 